﻿using System;
using System.Collections.Generic;
using System.Text;

namespace LocalizationTest
{
  class odometry : SelfLocator
  {
    Type currentWorldStateType = null;
    WorldState currentState = null;
    WorldState lastSimulatedState = null;
    int currentIndex = 0;

    List<WorldState> kidnapList = new List<WorldState>();

    private void Init(WorldState sampleWorldState)
    {
      currentWorldStateType = sampleWorldState.GetType();
      currentState = (WorldState)sampleWorldState.Clone();
      lastSimulatedState = (WorldState)sampleWorldState.Clone();
      currentIndex = 0;
      kidnapList.Clear();
    }

    public override WorldState Execute(Experiment experiment)
    {
      if (experiment.simulatedWorldState.GetType() != currentWorldStateType) Init(experiment.simulatedWorldState);
      if (experiment.actionAndObservationHistory.history.Count < currentIndex) Init(experiment.simulatedWorldState);

      WorldState lastSimulatedState2 = null;
      while (currentIndex < experiment.actionAndObservationHistory.history.Count)
      {
        if (experiment.actionAndObservationHistory.history[currentIndex] is ActionDescription)
        {
          ActionDescription currentAction = experiment.actionAndObservationHistory.history[currentIndex] as ActionDescription;
          currentState = currentAction.ExecutePerfect(experiment.motionModel, currentState);
          lastSimulatedState2 = currentAction.ExecutePerfect(experiment.motionModel, lastSimulatedState);
        }
        currentIndex++;
      }
      double[] lastStateAsDouble;
      if (lastSimulatedState2 != null)
        lastStateAsDouble = lastSimulatedState2.ToDoubleArray();
      else
        lastStateAsDouble = lastSimulatedState.ToDoubleArray();

      double[] currentStateAsDouble = experiment.simulatedWorldState.ToDoubleArray();
      for (int i = 0; i < lastStateAsDouble.Length; i++)
      {
        if (Math.Abs(lastStateAsDouble[i] - currentStateAsDouble[i]) > 20)
          kidnapList.Add(lastSimulatedState);
        else if (i == 2 && 0.3 <= Math.Abs(lastStateAsDouble[i] - currentStateAsDouble[i]))
          kidnapList.Add(lastSimulatedState);
      }
      lastSimulatedState = (WorldState)experiment.simulatedWorldState.Clone();

      foreach (WorldState worldState in kidnapList) worldState.DrawHighlighted("world:kidnappings", System.Drawing.Color.Red);

      return currentState;
    }
  }
}
